/*
 * openmv.c
 *
 *  Created on: May 26, 2021
 *      Author: kirk_
 */

#include "openmv.h"
#include "servo.h"

volatile uint8_t  control_s;  /* 0 turn, 1 go staight */
volatile uint8_t  control_t;  /* 0 turn left, 1 turn right */
volatile uint8_t  control_c;  /* 0 not found, 1 45 degree turn, 2 90 degree, 3 135 degree */
volatile uint16_t control_x;  /* horizontal position, 128 is center */
volatile uint16_t control_y;  /* vertical position, 128 is center */
volatile uint16_t control_r;  /* Radius */
volatile uint8_t  control_n;  /* new or old, when received convert to 1, when used convert to 0 */

void OpenMV1(uint8_t s, uint8_t t)
{
	control_s = s;
	control_t = t;
	control_n |= (uint8_t)1<<OpenMV_Task_Line;
	osThreadFlagsSet(Patio1TaskHandle, (uint32_t)1<<OpenMV_Task_Line);
}

void OpenMV2(uint8_t c)
{
	control_c = c;
	control_n |= (uint8_t)1<<OpenMV_Task_Color;
	osThreadFlagsSet(Patio2TaskHandle, (uint32_t)1<<OpenMV_Task_Color);
}

void OpenMV3(uint16_t x, uint16_t y, uint16_t r)
{
	control_x = x;
	control_y = y;
	control_r = r;
	control_n |= (uint8_t)1<<OpenMV_Task_Block;
	osThreadFlagsSet(Patio2TaskHandle, (uint32_t)1<<OpenMV_Task_Block);
	osThreadFlagsSet(Patio1TaskHandle, (uint32_t)1<<OpenMV_Task_Block);
}

void OpenMVLog(void)
{
	printf("\r\n");
	printf("[OpenMV] s %u.\r\n", control_s);
	printf("[OpenMV] t %u.\r\n", control_t);
	printf("[OpenMV] c %u.\r\n", control_c);
	printf("[OpenMV] x %u.\r\n", control_x);
	printf("[OpenMV] y %u.\r\n", control_y);
	printf("[OpenMV] n %u.\r\n", control_n);
}

void OpenMVCtl(uint8_t command)
{
	switch(command)
	{
	case 1:
		/* Find line */
		OpenCtl0_GPIO_Port->ODR &= ~OpenCtl0_Pin;
		OpenCtl1_GPIO_Port->ODR &= ~OpenCtl1_Pin;
		break;
	case 2:
		/* Identify color */
		OpenCtl0_GPIO_Port->ODR &= ~OpenCtl0_Pin;
		OpenCtl1_GPIO_Port->ODR |= OpenCtl1_Pin;
		break;
	case 3:
		/* Block */
		OpenCtl0_GPIO_Port->ODR |= OpenCtl0_Pin;
		OpenCtl1_GPIO_Port->ODR &= ~OpenCtl1_Pin;
		break;
	default:
		/* Find red block */
		OpenCtl0_GPIO_Port->ODR |= OpenCtl0_Pin;
		OpenCtl1_GPIO_Port->ODR |= OpenCtl1_Pin;
		break;
	}
}

void OpenMVReset(void)
{
	OpenRST_GPIO_Port->ODR &= ~OpenRST_Pin;
}

void OpenMVSet(void)
{
	OpenRST_GPIO_Port->ODR |= OpenRST_Pin;
}

void OpenMVFront(void)
{
	Servo_Turn(0, 108);
}

void OpenMVDown(void)
{
	Servo_Turn(0, 28);
}
